#include "ProgramOptions.h"
#include "MCG_dataset.h"
//#include "HelperFunctions.h"

void ProgramOptions::getImagePaths( vector<vector<string>> & filesPerCamera )
{
#ifndef USE_OLD_FILESYSTEM
	getFilesPerCamera( filesPerCamera );
#else
	getFilesPerCamera_old( filesPerCamera );
#endif
}

void ProgramOptions::getFilesPerCamera( vector<vector<string>> & files )
{
	namespace MCG = datasets::MCG;
	namespace bf = boost::filesystem;
	bf::path dirPath( image_path, bf::native );
	if ( !bf::exists( dirPath ) )
	{
		return;
	}

	MCG::MCGDataset fileList(dirPath.string(), begin_frame, begin_frame + num_of_frames );
	for( int i = 0 ; i < num_of_cams; ++i)
	{
		vector<bf::path> oneCameraList;
		fileList.list_imagefiles(oneCameraList, vbl::math::power(2,i)); // TODO use int to flag of camera

		BOOST_FOREACH( bf::path& path, oneCameraList )
		{
			files[i].push_back(path.string());
		}
	}
}

void ProgramOptions::getFilesPerCamera_old( vector< vector< string > >& files ) 
{
	namespace bf = boost::filesystem;
	bf::path dirPath( image_path, bf::native );
	if ( !bf::exists( dirPath ) )
	{
		return;
	}
	
	const boost::regex my_filter( "(07|14|19)_([0-9]+)_[0-9]+\\.[0-9]+\\.raw" );
	bf::directory_iterator itrEnd;
	for ( bf::directory_iterator itr( dirPath ); itr != itrEnd; ++itr )
	{
		if ( !bf::is_directory( *itr ) )
		{
			boost::smatch match;
			
			string& fileName = itr->leaf();

			if( boost::regex_match( fileName, match, my_filter ) )
			{
				int cam;
				
				// match camera number
				try
				{
					string s_match(match[1].first, match[1].second);
					cam = lexical_cast<int>(s_match); // string to int conversion					
				}
				catch(const bad_lexical_cast &)
				{
					// ok, couldn't find the camera number, trying to interpret an empty string
					// continue with next file
					continue;
				}

				switch( cam )
				{
					case 14: files[0].push_back( fileName ); break;
					case 19: files[1].push_back( fileName ); break;
					case 7: files[2].push_back( fileName ); break;
				}
			}			
		}
	}
}

//template <typename T, template <class> class AP1>
void ProgramOptions::undistortImage( img::ImageRGB & capuredImage, const util::Array2D< double >& mapXMat, 
									const util::Array2D< double >& mapYMat )
{
	img::ImageRGB undistortedImage;	
	undistortImage( capuredImage, undistortedImage, mapXMat, mapYMat );
	capuredImage = undistortedImage;
}

//template <typename T, template <class> class AP1, template <class> class AP2>
void ProgramOptions::undistortImage( const img::ImageRGB & capuredImage, img::ImageRGB & undistortedImage, 
									const util::Array2D< double >& mapXMat, const util::Array2D< double >& mapYMat )
{
	undistortedImage.resize( capuredImage.size() );
	
	// we need to adapt the access policy of the input image for the
	// undistort() method as the interpolation may try to sample outside the image boundries
	typedef img::Image< img::Triple<>, img::BorderedAccess > BorderedAccessImage;
	BorderedAccessImage imProxy = img::wrapImage< img::BorderedAccess >( capuredImage );
	BorderedAccessImage imProxy2 = img::wrapImage< img::BorderedAccess >( undistortedImage );
	img::proc::undistort< img::InterpolatorBilinear >( imProxy, imProxy2, mapXMat, mapYMat );
}

void ProgramOptions::computeUndistortMap( const calib::BogData& bogData, util::Array2D< double >& mapXMat, 
										 util::Array2D< double >& mapYMat	)
{
	double fx = bogData.intr.fc_u;
	double fy = bogData.intr.fc_v;
	double cx = bogData.intr.cc_u;
	double cy = bogData.intr.cc_v;
	double k1 = bogData.intr.kc_1;
	double k2 = bogData.intr.kc_2;
	double p1 = bogData.intr.kc_3;
	double p2 = bogData.intr.kc_4;
	img::proc::makeUndistortMap( mapXMat, mapYMat, fx, fy, cx, cy, k1, k2, p1, p2, true );	
}

void ProgramOptions::computeCameraParameters()
{
	const int num_of_cams_ = 4; // DOUWE/TODO: How to set a const from somewhere else?!

	vector< util::Array2D< double > > dist_map_x;
	vector< util::Array2D< double > > dist_map_y;

	dist_map_x.resize( num_of_cams, util::Array2D< double >( image_width, image_height ) );
	dist_map_y.resize( num_of_cams, util::Array2D< double >( image_width, image_height ) );
	
	calib::BogData bog_data[num_of_cams_];
	readBogData( bog_path, bog_data );

	for( int cam = 0; cam < num_of_cams ; ++cam )
	{
		computeUndistortMap( bog_data[cam], dist_map_x[cam], dist_map_y[cam] );
	}

	dist_map = make_pair( dist_map_x, dist_map_y);
}

void ProgramOptions::parseLine( string line )
{
	size_t recPos = line.find("RECTANGLE");
	if ( recPos == string::npos ) 
	{
		// contains no rectangle information
		return;
	} 
	else 
	{
		rectangle r;
		// contains rectangle information
		size_t firstSpacePos = line.find( " " );
		size_t secondSpacePos = line.find( " " , firstSpacePos + 1 );
		size_t thirdSpacePos = line.find( " " , secondSpacePos + 1 );
			
		string cam = line.substr( firstSpacePos + 1 , secondSpacePos - firstSpacePos - 1 );
		string loc = line.substr( secondSpacePos + 1 , thirdSpacePos - secondSpacePos - 1 );
		string rec = line.substr( thirdSpacePos + 1 , line.size());

		int camNr = lexical_cast<int>( cam );
		int locNr = lexical_cast<int>( loc );

		if ( rec == "notvisible" )
		{
			r.isVisible = false;
			r.xmin = -1;
			r.xmax = -1;
			r.ymin = -1;
			r.ymax = -1;
		} 
		else 
		{
			r.isVisible = true;
			size_t xminLoc = rec.find( " " );
			size_t yminLoc = rec.find( " " , xminLoc + 1 );
			size_t xmaxLoc = rec.find( " " , yminLoc + 1 );

			string xminStr = rec.substr( 0 , xminLoc );
			string yminStr = rec.substr( xminLoc + 1 , yminLoc - xminLoc - 1 );
			string xmaxStr = rec.substr( yminLoc + 1 , xmaxLoc - yminLoc - 1 );
			string ymaxStr = rec.substr( xmaxLoc + 1 , rec.size() );

			r.xmin = lexical_cast<int>( xminStr );
			r.ymin = lexical_cast<int>( yminStr );
			r.xmax = lexical_cast<int>( xmaxStr );
			r.ymax = lexical_cast<int>( ymaxStr );
		}

		pom_locations[ camNr ][ locNr ] = r;
	}

}

void ProgramOptions::readPomFile()
{
	ifstream file(pom_file.c_str());

	if(!file.is_open())
	{
		throw vbl::runtime_error( "[readPomFile] POM file not found:" + pom_file );
	} 
	else 
	{
		string line;
		while(getline(file, line))
		{
			parseLine( line );
		}
		file.close();
	}
}

void ProgramOptions::readConfigFile(string config_file_path, string & scenario_name, string & proba_path, string & image_path, 
					string & background_path, string & pom_file, string & output_path, string & bog_path, int & pom_x, 
					int & pom_y, int & begin_frame, int & num_of_frames, int & batch_size, double & passProb)
{
	ifstream config_file(config_file_path.c_str());
	bool sn = false, pp = false, ip = false, bp = false, pf = false, op = false, px = false, py = false, bf = false, 
		nof = false, bs = false, bogp = false, pap = false;

	if(!config_file.is_open())
	{
		throw vbl::runtime_error( "[readConfigFile] Config file not found:" + config_file_path );
	} 
	else 
	{
		static enum StringValue 
		{
			eSN,
			ePP,
			eIP,
			eBP,
			ePF,
			eOP,
			ePX,
			ePY,
			eBF,
			eNOF,
			eBS,
			eBOGP,
			ePAP
		};

		static std::map<std::string, StringValue> entryMap;
		entryMap["SCENARIONAME"] = eSN;
		entryMap["PROBAPATH"] = ePP;
		entryMap["IMAGEPATH"] = eIP;
		entryMap["BACKGROUNDPATH"] = eBP;
		entryMap["POMFILE"] = ePF;
		entryMap["OUTPUTPATH"] = eOP;
		entryMap["POMX"] = ePX;
		entryMap["POMY"] = ePY;
		entryMap["BEGINFRAME"] = eBF;
		entryMap["NUMOFFRAMES"] = eNOF;
		entryMap["BATCHSIZE"] = eBS;
		entryMap["BOGPATH"] = eBOGP;
		entryMap["PASSPROBABILITY"] = ePAP;

		string line;
		while(getline(config_file, line))
		{
			size_t space_position = line.find( " " );
			string config_line = line.substr( 0 , space_position );
			space_position++;
			switch ( entryMap[config_line] )
			{
				case eSN:
					scenario_name = line.substr( space_position , line.size() );
					sn = true;
					break;
				case ePP:
					proba_path = line.substr( space_position , line.size() );
					pp = true;
					break;
				case eIP:
					image_path = line.substr( space_position , line.size() );
					ip = true;
					break;
				case eBP:
					background_path = line.substr( space_position , line.size() );
					bp = true;
					break;
				case ePF:
					pom_file = line.substr( space_position , line.size() );
					pf = true;
					break;
				case eOP:
					output_path = line.substr( space_position , line.size() );
					op = true;
					break;
				case ePX:
					pom_x = lexical_cast<int>( line.substr( space_position , line.size() ) );
					px = true;
					break;
				case ePY:
					pom_y = lexical_cast<int>( line.substr( space_position , line.size() ) );
					py = true;
					break;
				case eBF:
					begin_frame = lexical_cast<int>( line.substr( space_position , line.size() ) );
					bf = true;
					break;
				case eNOF:
					num_of_frames = lexical_cast<int>( line.substr( space_position , line.size() ) );
					nof = true;
					break;
				case eBS:
					batch_size = lexical_cast<int>( line.substr( space_position , line.size() ) );
					bs = true;
					break;
				case eBOGP:
					bog_path = lexical_cast<string>( line.substr( space_position , line.size() ) );
					bogp = true;
					break;
				case ePAP:
					passProb = lexical_cast<double>( line.substr( space_position , line.size() ) );
					pap = true;
					break;
				default:
					break;
			}
		}
		if ( sn == false || pp == false || ip == false || bp == false || pf == false || op == false || px == false 
			|| py == false || bf == false || nof == false || bs == false || bogp == false || pap == false )
		{
			throw vbl::runtime_error( "[readConfigFile] Not all neccesary fields found in file." );
		}
	}
}

void ProgramOptions::readBogData( const string& bogDir,	calib::BogData* bogData	)
{
	for ( size_t i = 0; ; ++i )
	{
		const string bogFilename( bogDir + "calib_k" + util::nrToString( i, 1 ) + ".bog" );
		if ( !util::fileExists( bogFilename ) )
			break;
		bogData[i] = calib::readBogFile( bogFilename );		
	}
}

// TODO: hacky, this is also in batch
void ProgramOptions::getAppearance( int frame, rectangle r, img::ImageRGB & im, img::ImageRGB & background, 
								   AppearanceHist & apHist )
{
	img::Triple<> maskColor(0,204,153);

	img::ImageRGB cutout( r.xmax - r.xmin, r.ymax - r.ymin );
	img::ImageRGB backgroundCutOut( r.xmax - r.xmin , r.ymax - r.ymin );

	img::copy( im, r.xmin, r.ymin, r.xmax - r.xmin, r.ymax - r.ymin, cutout);
	img::copy( background , r.xmin , r.ymin , r.xmax - r.xmin , r.ymax - r.ymin , backgroundCutOut );
/*
	util::Window win("Cut out");
	win.showImage(backgroundCutOut);
	win.waitForKey();
*/
	apHist.add( cutout, backgroundCutOut, maskColor );
	apHist.normalize();
}

void ProgramOptions::getAppearance( int frame, int location, vector<img::ImageRGB> & ims, vector<AppearanceHist> & appearance,
								   vector<bool> & visibility )
{
	for( int cam = 0 ; cam < num_of_cams ; cam++ )
	{
		rectangle r = pom_locations[cam][location-1];
		if( r.isVisible )
		{
			img::ImageRGB background;
#ifndef USE_CAS_DATASET
			string backgroundImPath = p_po->background_path + lexical_cast<string>(frame) + "_" + lexical_cast<string>(cam) + 
				"-result.png";
#else
			string backgroundImPath = background_path + "result-f" + lexical_cast<string>(frame) + "-c" + 
				lexical_cast<string>(cam) + ".png";
#endif
			try
			{
				img::io::read(backgroundImPath , background );
			} 
			catch( vbl::runtime_error e ) 
			{
				cout << " Could not read background file: " << e.what() << endl;
				exit(1);
			}
			AppearanceHist apHist;
			getAppearance( frame, r, ims[cam], background, apHist );
			appearance.push_back(apHist);
			visibility.push_back( true );
		}
		else
		{
			AppearanceHist apHist;
			appearance.push_back(apHist);
			visibility.push_back( false );
		}
	}
}

void ProgramOptions::getImages( int frame, vector<img::ImageRGB> & ims_out )
{
	vector<img::ImageRGB> ims;
	for( int cam = 0; cam < num_of_cams ; ++cam )
	{
		img::ImageG imRaw( image_width, image_height );
#ifndef USE_OLD_FILESYSTEM
		img::io::readRAW( files_per_camera[cam][ frame ], imRaw, imRaw.width(), imRaw.height() ); 
#else
		img::io::readRAW( image_path + "/" + files_per_camera[cam][ begin_frame + frame ], imRaw, imRaw.width(), imRaw.height() ); 
#endif
		img::ImageRGB im;
		img::proc::demosaicPixelGrouping( imRaw, im );
		undistortImage( im, dist_map.first[cam], dist_map.second[cam] );
		img::rotateSafe< img::InterpolatorBilinear >( im, image_width/2, image_height/2, rot[cam] );
		ims.push_back(im);
	}
	ims_out = ims;
}

// TODO: end hacky

int ProgramOptions::calculateSurface( rectangle r ) 
{
	return ( r.xmax - r.xmin ) * ( r.ymax - r.ymin );
}

void ProgramOptions::setupTemplates()
{
	int num_of_people = 3;
	int locs[3] = { 1576, 1077, 1226 };
	int frame = 259;
	// 7-1 4 people, frame 30, locs: 557, 602, 793, 1143
	// 1-1 3 people, frame 259, locs: 1576, 1077, 1226
	vector<img::ImageRGB> ims;		  
	getImages( frame, ims );		  

	for( int i = 0 ; i < num_of_people ; i++ )
	{
		vector<AppearanceHist> appearance;
		vector<bool> visibility;
		getAppearance( begin_frame+frame, locs[i], ims, appearance, visibility );
		templates.push_back( appearance );
		camera_visibility.push_back( visibility );
	}
}

int ProgramOptions::calcOverlap( rectangle curr, rectangle comp )
{
	if( !comp.isVisible )
		return 0;

	if( curr.xmax < comp.xmin || curr.xmin > comp.xmax || curr.ymax < comp.ymin || curr.ymin > comp.ymax )
		return 0;

	bool left = curr.xmin > comp.xmin;
	bool right = curr.xmax < comp.xmax;
	bool top = curr.ymin > comp.ymin;
	bool bottom = curr.ymax < comp.ymax;

	rectangle overlap;
	overlap.isVisible = true;
	
	if( left )
		overlap.xmin = curr.xmin;
	else
		overlap.xmin = comp.xmin;

	if( right )
		overlap.xmax = curr.xmax;
	else
		overlap.xmax = comp.xmax;

	if( top )
		overlap.ymin = curr.ymin;
	else
		overlap.ymin = comp.ymin;

	if( bottom )
		overlap.ymax = curr.ymax;
	else
		overlap.ymax = comp.ymax;

	return calculateSurface( overlap );
}

void ProgramOptions::computeOcclusionMap()
{
	for( int cam = 0 ; cam < pom_locations.size() ; cam++ )
	{
		vector<rectangle> pom_loc_cam = pom_locations[cam];
		for( int loc = 0 ; loc < pom_loc_cam.size() ; loc++ )
		{
			rectangle curr = pom_loc_cam[loc];
			if( curr.isVisible )
			{
				vector<int> occlusions;
				int surface = calculateSurface( curr );
				for( int loc2 = 0 ; loc2 < pom_loc_cam.size() ; loc2++ )
				{
					if( loc != loc2 )
					{
						rectangle comp = pom_loc_cam[loc2];
						int comp_surface = calculateSurface( comp );
						if( comp_surface >= surface )
						{
							int overlap = calcOverlap( curr, comp );
							if( overlap != 0 && ((double)overlap/(double)surface) > 0.05 )
							{
								occlusions.push_back( loc2 + 1 );
							}
						}
					}
				}
				occlusion_map[ cam ][ loc + 1 ] = occlusions;
			}
		}
	}
}

ProgramOptions::ProgramOptions( string config_file_path )
{
	try
	{
		ProgramOptions::readConfigFile(config_file_path, scenario_name, proba_path, image_path, background_path, pom_file, 
			output_path, bog_path, pom_x, pom_y, begin_frame, num_of_frames, batch_size, pass_prob );
	}
	catch( vbl::runtime_error e )
	{
		cout << e.what() << endl;
		exit(-1);
	}

# ifndef USE_CAS_DATASET
	rot.push_back( 2.9477621582585201 );
	rot.push_back( -1.6764118889390500 );
	rot.push_back( -5.7642762038006197 );
	rot.push_back( 2.3607981090325212 );
	image_width = 1024;
	image_height = 768;
	num_of_cams = 4;
#else
	rot.push_back( -20.3973 );
	rot.push_back( -5.82713 );
	rot.push_back( -2.24835 );
	image_width = 752;
	image_height = 560;
	num_of_cams = 3;
#endif

	vsource = 0;
	max_detections = 25;
	b_save_images = true;
	use_appearance = false;
	first_graph = true;
	use_appearance_shitrit = true;
	output_path = output_path + scenario_name + "/";
	pom_size = pom_x * pom_y;
	num_of_batches = int(ceil((double(num_of_frames)/double(batch_size))));
	total_locations = pom_size * num_of_frames + 2; // is + 2 because of the addition of vsource and vsink

	for (int i = 0 ; i < num_of_cams ; i++ ) 
	{
		vector<rectangle> toAdd ( pom_size );
		pom_locations.push_back( toAdd );
		map<int,vector<int>> toAdd2;
		occlusion_map.push_back( toAdd2 );
	}

	readPomFile();
	computeOcclusionMap();

	computeCameraParameters();
	
	vector<vector<string>> files_per_camera_( num_of_cams );
	getImagePaths( files_per_camera_ );
	files_per_camera = files_per_camera_;

	if( use_appearance_shitrit )
	{
		setupTemplates();
	}


}		   